#include "ObstacleAvoidanceBehav.h"

ObstacleAvoidanceBehav::ObstacleAvoidanceBehav(){
}

ObstacleAvoidanceBehav::ObstacleAvoidanceBehav(float aMin,float aMax,int nBeam,float K){
  _angleMin=aMin;
  _angleMax=aMax;
  _nBeam=nBeam;
  _K=K;
}

ObstacleAvoidanceBehav::ObstacleAvoidanceBehav(const ObstacleAvoidanceBehav& orig) {
}

ObstacleAvoidanceBehav::~ObstacleAvoidanceBehav() {
}

geometry_msgs::Twist ObstacleAvoidanceBehav::run(sensor_msgs::LaserScan& scan){
  geometry_msgs::Twist cmd;
  cmd.linear.x=0;
  cmd.linear.y=0;
  cmd.linear.z=0;
  cmd.angular.x=0;
  cmd.angular.y=0;
  cmd.angular.z=0;
  
  float tetaMin[_nBeam],dMin[_nBeam];
  for(int i=0;i<_nBeam;i++){
      tetaMin[i]=_angleMin+((_angleMax-_angleMin)/_nBeam)*i;
      dMin[i]=0;
    }
  
  double Ds=0.10;
  for (float a = _angleMin; a < _angleMax; a+=scan.angle_increment) {
    int index=int((a-_angleMin)/((_angleMax-_angleMin)/_nBeam));//int(((a+M_PI/2)*180/M_PI)/(180/nBeam));
    if (scan.ranges[aToRange(a,scan)] <= scan.ranges[aToRange(tetaMin[index],scan)] && scan.ranges[aToRange(a,scan)]>scan.range_min && scan.ranges[aToRange(a,scan)]<scan.range_max) {
      dMin[index]=scan.ranges[aToRange(a,scan)];
      tetaMin[index]=a;
    }
  }
  for(int i=0;i<_nBeam;i++){
    double p=(1+Ds)/(1+dMin[i]);
    double x=(int(floor(p))%2==0)?p-floor(p):p-ceil(p);
    cmd.angular.z += -(tetaMin[i])*exp(-(_K*dMin[i])-((tetaMin[i]*tetaMin[i])/(2*asin(x)*asin(x))));
  }
  return cmd;
}

int ObstacleAvoidanceBehav::aToRange(float a,sensor_msgs::LaserScan& scan){
    return int((a-scan.angle_min)/scan.angle_increment);
  }